// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*!
 * @file SensorCombinedSubscriber.cpp
 * This file contains the implementation of the subscriber functions.
 *
 * This file was generated by the tool fastcdrgen.
 */

#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/attributes/SubscriberAttributes.h>

#include <fastrtps/Domain.h>

#include "SensorCombinedSubscriber.h"


SensorCombinedSubscriber::SensorCombinedSubscriber() : mp_participant(nullptr), mp_subscriber(nullptr) {}

SensorCombinedSubscriber::~SensorCombinedSubscriber() {	Domain::removeParticipant(mp_participant);}

bool SensorCombinedSubscriber::init()
{
	// Create RTPSParticipant

	ParticipantAttributes PParam;
	PParam.rtps.builtin.domainId = 0; //MUST BE THE SAME AS IN THE PUBLISHER
	PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
	PParam.rtps.setName("Participant_subscriber"); //You can put the name you want
	mp_participant = Domain::createParticipant(PParam);
	if(mp_participant == nullptr)
			return false;

	//Register the type

	Domain::registerType(mp_participant,(TopicDataType*) &myType);

	// Create Subscriber

	SubscriberAttributes Rparam;
	Rparam.topic.topicKind = NO_KEY;
	Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
	Rparam.topic.topicName = "SensorCombinedPubSubTopic";
	mp_subscriber = Domain::createSubscriber(mp_participant,Rparam,(SubscriberListener*)&m_listener);
	if(mp_subscriber == nullptr)
		return false;
	return true;
}

void SensorCombinedSubscriber::SubListener::onSubscriptionMatched(Subscriber* sub,MatchingInfo& info)
{
	if (info.status == MATCHED_MATCHING)
	{
		n_matched++;
		cout << "Subscriber matched" << endl;
	}
	else
	{
		n_matched--;
		cout << "Subscriber unmatched" << endl;
	}
}

void SensorCombinedSubscriber::SubListener::onNewDataMessage(Subscriber* sub)
{
	// Take data
	SensorCombined sensor_data;

	if(sub->takeNextData(&sensor_data , &m_info))
	{
		if(m_info.sampleKind == ALIVE)
		{
            cout << "\n\n\n\n\n\n\n\n\n\n";
            cout << "Received sensor_combined data" << endl;
            cout << "=============================" << endl;
            cout << "timestamp: " << sensor_data.timestamp() << endl;
            cout << "gyro_rad: " << sensor_data.gyro_rad().at(0);
            cout << ", " << sensor_data.gyro_rad().at(1);
            cout << ", " << sensor_data.gyro_rad().at(2) << endl;
            cout << "gyro_integral_dt: " << sensor_data.gyro_integral_dt() << endl;
            cout << "accelerometer_timestamp_relative: " << sensor_data.accelerometer_timestamp_relative() << endl;
            cout << "accelerometer_m_s2: " << sensor_data.accelerometer_m_s2().at(0);
            cout << ", " << sensor_data.accelerometer_m_s2().at(1);
            cout << ", " << sensor_data.accelerometer_m_s2().at(2) << endl;
            cout << "accelerometer_integral_dt: " << sensor_data.accelerometer_integral_dt() << endl;
            cout << "magnetometer_timestamp_relative: " << sensor_data.magnetometer_timestamp_relative() << endl;
            cout << "magnetometer_ga: " << sensor_data.magnetometer_ga().at(0);
            cout << ", " << sensor_data.magnetometer_ga().at(1);
            cout << ", " << sensor_data.magnetometer_ga().at(2) << endl;
            cout << "baro_timestamp_relative: " << sensor_data.baro_timestamp_relative() << endl;
            cout << "baro_alt_meter: " << sensor_data.baro_alt_meter() << endl;
            cout << "baro_temp_celcius: " << sensor_data.baro_temp_celcius() << endl;
		}
	}
}

void SensorCombinedSubscriber::run()
{
	cout << "Waiting for Data, press Enter to stop the Subscriber. "<<endl;
	std::cin.ignore();
	cout << "Shutting down the Subscriber." << endl;
}
